#include "attachRev.h"
#include "simpleSocket.h"
#include "../proto/rawdata.pb.h"

#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <errno.h>
#include <time.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <libusb-1.0/libusb.h>
#include <list>
#include <map>
#include <signal.h>

using std::list;
using std::map;

#define GET_NEXT_PACKET (0x13)
#define MAX_READ_SIZE (64 * 1024) 

#define SERV_PORT 7005

#define RSSI_OFFSET 78

#define DEBUG 0

typedef struct {
    unsigned char ex_length : 8;
    unsigned char dropped : 8;
    unsigned int boardID : 24;
    unsigned int time : 32;
    unsigned int tagID : 21;
    unsigned int parity : 3;
    unsigned char rssi : 8;
    unsigned char status : 8;
    unsigned char data[20];
} __attribute__((packed)) packet_t;

const int PACKET_LEN = 13;

unsigned int getTransID(unsigned char * data) {

    return ((unsigned int)data[9] * 8192)  + ((unsigned int)data[10] * 32) +
        ((unsigned int)data[11] >> 3);
}

unsigned long getReceiveID(unsigned char * data) {
    packet_t *pt = (packet_t *)data;

    return ntohl(pt->boardID << 8);
}

uint64_t getDataTime(unsigned char * data) {

    timeval time;
    gettimeofday(&time, NULL);
    long long sec = time.tv_sec;
    //32bit multi 1000 will be overflow
    return time.tv_usec / 1000 + sec * 1000;

}

double getRssi(unsigned char * data) {
    packet_t * pt = (packet_t *)data;
    double rssi = (pt->rssi >= 128 ?
            (signed int)(pt->rssi - 256) / 2.0
            : pt->rssi / 2.0 ) - RSSI_OFFSET;

    return rssi;

}

int main(int argc, char ** argv) {

    if(argv[1] == NULL){
        fprintf(stderr, "Useage: senlayer <IP>\n");
        return -1;
    }

    signal(SIGPIPE, SIG_IGN);

    int sockfd = socket_init(argv[1], SERV_PORT);
    while(sockfd == -1){
        fprintf(stderr, "connect agg server failed ,retry..\n");
        sockfd = socket_init(argv[1],SERV_PORT);
        sleep(3);
    }

    list<libusb_device_handle*> rev_handles;
    libusb_init(NULL);
    libusb_set_debug(NULL, 3);

    attachRev(rev_handles);
    //printf("%d receives had been found\n", rev_handles.size());

    unsigned char msg[128];
    msg[0] = GET_NEXT_PACKET;

    unsigned char buf[MAX_READ_SIZE];

    memset(buf, 0, MAX_READ_SIZE);

    int transferred = -1;

    map<unsigned long, bool> found_trans;


    while (true) {

        try {
            attachRev(rev_handles);
            rev_handles.sort();
            rev_handles.unique();

            //usleep(1000000);
            //fprintf(stderr, "%d receives is using \n", rev_handles.size());

            for (list<libusb_device_handle*>::iterator I = rev_handles.begin(); I != rev_handles.end(); ++I) {

                int retries = 2;
                bool read_success = false;

                while (read_success == false && retries > 0) {

                    //fprintf(stderr, "retries number %d\n", retries);

                    int retbulk = libusb_bulk_transfer(*I, 2 | LIBUSB_ENDPOINT_OUT, msg, 1, &transferred, 0);

                    if (retbulk != 0) {
                        fprintf(stderr, "Error request from usb, %s \n", strerror(retbulk));
                        --retries;
                    } else {

                        retbulk = libusb_bulk_transfer(*I, 1 | LIBUSB_ENDPOINT_IN, buf + 1, PACKET_LEN + 20, &transferred, 0);

                        if (retbulk != 0) {
                            fprintf(stderr, "Error input from usb, %s \n", strerror(retbulk));
                            --retries;
                        } else {
                            read_success = true;
                            buf[0] = transferred - PACKET_LEN;
                            //fprintf(stderr, "get_pack num %d \n", transferred);
                        }
                    }

                }

                if (retries == 0) {
                    libusb_release_interface(*I, 0);
                    libusb_close(*I);
                    *I = NULL;
                }

                if (read_success == true) {
                    if (PACKET_LEN <= transferred) {

                        //fprintf(stderr, "get packet \n");
                        packet_t * pkt = (packet_t*)buf;
                        if (pkt->rssi != 0 && pkt->status != 0) {

                            //  fprintf(stderr, "get good packet \n");
                            unsigned char * data = (unsigned char *)pkt;

                            bool parity_failed = false;
                            {
                                unsigned char p1 = 0;
                                unsigned char p2 = 0;
                                unsigned char p3 = 0;
                                unsigned long packet = ((unsigned int)data[9]  << 16) |
                                    ((unsigned int)data[10] <<  8) |
                                    ((unsigned int)data[11]);

                                int i;
                                /* XOR each group of 3 bytes until all of the 24 bits have been XORed. */
                                for (i = 7; i >= 0; --i) {
                                    unsigned char triple = (packet >> (3 * i)) & 0x7;
                                    p1 ^= triple >> 2;
                                    p2 ^= (triple >> 1) & 0x1;
                                    p3 ^= triple & 0x1;
                                }
                                /* If the end result of the XORs is three 0 bits then even parity held,
                                 * which suggests that the packet data is good. Otherwise there was a bit error. */
                                if (p1 ==  0 && p2 == 0 && p3 == 0) {
                                    parity_failed = false;
                                }
                                else {
                                    parity_failed = true;
                                }
                            }

                            if (parity_failed == false) {

                                uint32_t Tx_ID = getTransID(data);
                                uint64_t Rx_ID = getReceiveID(data);
                                uint64_t timestamp = getDataTime(data);
                                double rss = getRssi(data);  

                                senslayer::Rawdata rawdata;
                                rawdata.set_timestamp(timestamp);
                                rawdata.set_rxid(Rx_ID);
                                rawdata.set_txid(Tx_ID);
                                rawdata.set_rss(rss);

                                std::string body;

                                rawdata.SerializeToString(&body);
                                //char * msg = "sent from sensor_layer\n";
                                char buff[512*1024];
                                memset(buff, 0, 512);

                                //使用四个字节作为头部,标识序列化后的字符串长度
                                uint32_t headersize = sizeof(uint32_t);

                                //序列化后数据的长度
                                uint32_t len = body.length();

                                //使用数据长度对数据头部进行赋值
                                memcpy(buff, &len, headersize);

                                //对传输的数据体进行值
                                body.copy(buff + headersize, len, 0);

                                int nw = send(sockfd, buff, headersize + body.length(),0);
                                fprintf(stderr, "send: %d\n", nw);
                                if(nw == -1){
                                    char * errormsg = strerror(errno);
                                    fprintf(stderr,"%s\n", errormsg);
                                    sleep(3);
                                    sockfd = -1;
                                    while(sockfd == -1){
                                        fprintf(stderr, "connect agg server failed ,retry..\n");
                                        close(sockfd);
                                        sockfd = socket_init(argv[1],SERV_PORT);
                                        sleep(3);
                                    }
                                }

                            }
                        }
                    }
                }
            }
            rev_handles.remove(NULL);

        } catch (std::exception &e) {
            fprintf(stderr, "attachRev error \n");
        }

        if (rev_handles.size() < 1) {
            fprintf(stderr, "no receives \n");
            usleep(1000000);
        }
    }


    for (list<libusb_device_handle*>::iterator I = rev_handles.begin(); I != rev_handles.end(); ++I) {
        libusb_release_interface(*I, 0);
        libusb_close(*I);
    }

    libusb_exit(NULL);
    socket_close(sockfd);

    return 0;

}

